# license MIT CelerStar
enable_lens_corr = False
import sensor, image, time, math

def rightAngle_init():
    sensor.reset()
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QQVGA)          #QVGA不行，报错
    #sensor.set_vflip(True)
    #sensor.set_auto_gain(False) #必须关闭以进行颜色跟踪 白平衡
    #sensor.set_auto_whitebal(False) #关闭 自动增益


MX = -1
MY = -1
DetectCnt = 0
clock = time.clock()
def detectRightAngle():
    global MX, MY, DetectCnt, clock
    clock.tick()
    img = sensor.snapshot()     #得到照片
    img.to_grayscale()
    img.mean(1)
    img.find_edges(image.EDGE_CANNY, threshold=(150, 200))
    CX = 0
    CY = 0
    PTgroup = []
    lines = img.find_lines( roi=(30,10,100,100) ,x_stride=1,y_stride=1,threshold = 1000, theta_margin = 5, rho_margin = 300)
    for i in range(0,len(lines)-1):
        for j in range(i+1,len(lines)):
            l0x1 = lines[i].x1()
            l0y1 = lines[i].y1()
            l0x2 = lines[i].x2()
            l0y2 = lines[i].y2()
            if(l0x1 == l0x2):
                l0x1 = l0x1 + 0.1
            a0 = (l0y2 - l0y1)/(l0x2 - l0x1)
            b0 = l0y1 - a0*l0x1
            l1x1 = lines[j].x1()
            l1y1 = lines[j].y1()
            l1x2 = lines[j].x2()
            l1y2 = lines[j].y2()
            if(l1x1 == l1x2):
                l1x1 = l1x1 + 0.1

            vx0 = l0x2-l0x1
            vy0 = l0y2-l0y1
            vx1 = l1x2-l1x1
            vy1 = l1y2-l1y1
            num = vx0*vx1+vy0*vy1
            den = math.sqrt( (vx0*vx0+vy0*vy0)*(vx1*vx1+vy1*vy1) )
            if abs(den)<0.001:
                continue

            cosV = abs(num/den);
            if( cosV>0.5 ):
                continue


            a1 = (l1y2 - l1y1)/(l1x2 - l1x1)
            b1 = l1y1 - a1*l1x1
            if(a0 == a1):
                a0 = a0 + 0.1
            intersectionx = (b1-b0)/(a0-a1)
            intersectiony = a0*intersectionx + b0
            #img.draw_cross(int(intersectionx), int(intersectiony),color = (128))
            PTgroup.append([intersectionx,intersectiony])
            #在图像上绘制一个十字。 您可以单独传递x，y或作为元组(x，y)传递。

            orthogonal = float(a0) * float(a1)
            CX = CX + intersectionx
            CY = CY + intersectiony
            #if(orthogonal > -1.3 and orthogonal < -0.7 ):
                      #img.draw_circle(int(intersectionx), int(intersectiony),10,color = (255, 0, 0))

    #for l in lines:
        #img.draw_line(l.x1(),l.y1(),l.x2(),l.y2(), color = (128))
        #print(l.pixels())
    #print("FPS %f" % clock.fps())
    num = len(PTgroup)
    print(num)
    if abs(num-20)<10:
        CX = CX/num
        CY = CY/num
        if MX<0:
            MX = CX
            MY = CY
        MX = 0.3*MX + 0.7*CX
        MY = 0.3*MY + 0.7*CY
        DetectCnt = min(10,DetectCnt+1)
        if DetectCnt>5:
            img.draw_cross(int(MX), int(MY),color = (128))
            return True
    else:
        DetectCnt = max(0,DetectCnt-1)
